#ifndef MERGED_CLOUD_HPP
#define MERGED_CLOUD_HPP

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <mutex>
#include <deque>

/**
 * @brief 点云拼接类，最多存储5帧点云，超过时自动删除最早帧
 */
class Merged_cloud {
public:
    /**
     * @brief 构造函数，默认最多存储5帧
     * @param max_frames 最大存储帧数，默认为5
     */
    Merged_cloud(size_t max_frames = 5) : max_frames_(max_frames), frame_count_(0) {
        merged_cloud_ = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
    }

    /**
     * @brief 析构函数
     */
    ~Merged_cloud() = default;

    /**
     * @brief 添加新的点云到拼接队列
     * @param laser_cloud 待添加的点云指针
     */
    void add_cloud(pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud) {
        if (!laser_cloud || laser_cloud->empty()) {
            return; // 忽略空点云
        }

        std::lock_guard<std::mutex> lock(mutex_);
        
        // 添加新帧
        cloud_frames_.push_back(laser_cloud);
        
        // 如果超过最大帧数，删除最早的一帧
        if (cloud_frames_.size() > max_frames_) {
            cloud_frames_.pop_front();
        }
        
        // 更新帧计数（当前实际存储的帧数）
        frame_count_ = cloud_frames_.size();
        
        // 重新拼接所有保留的点云
        merge_all_frames();
    }

    /**
     * @brief 获取当前存储的点云帧数
     * @return 点云帧数（0到max_frames_之间）
     */
    int get_fram_sum(void) {
        std::lock_guard<std::mutex> lock(mutex_);
        return frame_count_;
    }

    /**
     * @brief 获取拼接后的点云指针（仅包含最新的max_frames_帧）
     * @return 拼接后的点云指针
     */
    pcl::PointCloud<pcl::PointXYZI>::Ptr get_merged_cloud(void) {
        std::lock_guard<std::mutex> lock(mutex_);
        return merged_cloud_;
    }

    /**
     * @brief 清空所有点云和计数
     */
    void clear() {
        std::lock_guard<std::mutex> lock(mutex_);
        frame_count_ = 0;
        cloud_frames_.clear();
        merged_cloud_->clear();
    }

    /**
     * @brief 设置最大存储帧数（可选功能）
     * @param max_frames 新的最大帧数限制
     */
    void set_max_frames(size_t max_frames) {
        std::lock_guard<std::mutex> lock(mutex_);
        max_frames_ = max_frames;
        
        // 如果当前帧数超过新的最大值，删除多余的最早帧
        while (cloud_frames_.size() > max_frames_) {
            cloud_frames_.pop_front();
        }
        
        frame_count_ = cloud_frames_.size();
        merge_all_frames(); // 重新拼接
    }

private:
    /**
     * @brief 合并当前所有保留的点云帧
     */
    void merge_all_frames() {
        // 清空现有拼接结果
        merged_cloud_->clear();
        
        // 合并所有帧
        for (const auto& frame : cloud_frames_) {
            *merged_cloud_ += *frame;
        }
    }

    std::mutex mutex_;                          // 线程安全锁
    size_t max_frames_;                         // 最大存储帧数（默认为5）
    int frame_count_;                           // 当前存储的帧数
    pcl::PointCloud<pcl::PointXYZI>::Ptr merged_cloud_;  // 拼接后的点云
    std::deque<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloud_frames_;  // 存储原始帧（按添加顺序）
};

#endif // MERGED_CLOUD_HPP


